# main.py
import cv2
import numpy as np
import struct
import serial
import time
import threading     

com = serial.Serial(
    port="/dev/ttyTHS1",
    baudrate=115200,
    bytesize=serial.EIGHTBITS,
    parity=serial.PARITY_NONE,
    stopbits=serial.STOPBITS_ONE,
)
time.sleep(1)

screen_width  = 640
screen_height = 480

roi_h = 220
roi_w = 260
roi_x = (screen_width  - roi_w) // 2
roi_y = (screen_height - roi_h) // 2

gray_threshold = 88

_latest_cx = None
_latest_cy = None
_lock = threading.Lock()

def _sender_1000hz():
    while True:
        with _lock:
            cx, cy = _latest_cx, _latest_cy
        if cx is not None:
            pkt = struct.pack("<BBhhB", 0x2C, 0x12, int(cx), int(cy), 0x5B)
            com.write(pkt)
        time.sleep(1.0/1000)          # 1000 Hz
threading.Thread(target=_sender_1000hz, daemon=True).start()
# -------------------------------------------------

def send_data(cx, cy):
    global _latest_cx, _latest_cy
    with _lock:
        _latest_cx, _latest_cy = int(cx), int(cy)

def run_camera_mode():
    global gray_threshold
    cap = cv2.VideoCapture(0)
    cap.set(cv2.CAP_PROP_FRAME_WIDTH,  screen_width)
    cap.set(cv2.CAP_PROP_FRAME_HEIGHT, screen_height)

    print("[CAMERA] Enter camera mode ...")
    try:
        while True:
            ret, frame = cap.read()
            if not ret:
                break

            cv2.rectangle(frame, (roi_x, roi_y),
                          (roi_x+roi_w, roi_y+roi_h), (0, 255, 0), 2)
            cv2.putText(frame, "ROI", (roi_x+10, roi_y+30),
                        cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)
            cv2.circle(frame, (screen_width//2, screen_height//2),
                       2, (0, 0, 255), -1)

            roi_frame = frame[roi_y:roi_y+roi_h, roi_x:roi_x+roi_w]
            gray_roi  = cv2.cvtColor(roi_frame, cv2.COLOR_BGR2GRAY)
            clahe = cv2.createCLAHE(clipLimit=2.0, tileGridSize=(8, 8))
            gray_roi = clahe.apply(gray_roi)
            _, th_roi = cv2.threshold(gray_roi, gray_threshold,
                                      255, cv2.THRESH_BINARY_INV)
            th_roi = cv2.morphologyEx(th_roi, cv2.MORPH_CLOSE,
                                      np.ones((3, 3), np.uint8), 1)

            border = 10
            th_roi[:border, :] = th_roi[-border:, :] = 0
            th_roi[:, :border] = th_roi[:, -border:] = 0

            contours, _ = cv2.findContours(th_roi, cv2.RETR_EXTERNAL,
                                           cv2.CHAIN_APPROX_SIMPLE)
            if contours:
                cnt = max(contours, key=cv2.contourArea)
                if cv2.contourArea(cnt) > 1000:
                    (cx, cy), (w, h), angle = cv2.minAreaRect(cnt)
                    cx += roi_x
                    cy += roi_y
                    box = cv2.boxPoints(cv2.minAreaRect(cnt)) + np.array([roi_x, roi_y])
                    cv2.polylines(frame, [np.int0(box)], True, (255, 0, 0), 2)
                    cv2.circle(frame, (int(cx), int(cy)), 5, (0, 0, 255), -1)
                    send_data(int(cx), int(cy))          
                    cv2.putText(frame, f"C:({int(cx)},{int(cy)})",
                                (int(cx)+10, int(cy)-10),
                                cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 0, 255), 2)

            cv2.imshow("ROI Processing", th_roi)
            cv2.moveWindow("ROI Processing", screen_width+10, 0)
            cv2.imshow("Main View", frame)
            cv2.moveWindow("Main View", 0, 0)

            if cv2.waitKey(5) & 0xFF == 27:
                break

    finally:
        cap.release()
        cv2.destroyAllWindows()
        print("[CAMERA] Exit camera mode.")

print("Waiting for serial command ...")
try:
    while True:

        run_camera_mode()

        # raw_data = com.read(5)
        # flag = raw_data[0]

        # if flag == 1:
        #     run_camera_mode()
        #     print("Waiting for serial command ...")

        # elif flag == 2:
        #     print("adjusting gray...")
        #     gray_threshold = raw_data[1]
        #     print(f"[PARAM] gray_threshold updated to {gray_threshold}")
        #     com.reset_input_buffer()
                

   

except KeyboardInterrupt:
    print("User abort.")
finally:
    com.close()